// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MultiPicknPlaceAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Back end for executing multi object pick and place from the CNC using the Kuka arm by turning it
    /// into a series of regular pick and place operations.
    /// The pick request is to move objects from the CNC to the table.
    /// The place request moves the objects back from the table to the CNC at its place position
    /// A separate FSM is built for each operation
    /// Note this is not generic as it relies on paraeters of the operating space
    /// </summary>
    public class MultiPicknPlace 
    {
        /// <summary>
        /// Minimum spacing between object on table for gripper clearance
        /// </summary>
        private readonly double minRequiredSpaceBetweenObjects = 0.320 / 2.0;

        /// <summary>
        /// Minimum Y spacing between placed objects
        /// </summary>
        private readonly double minRequiredYSpaceBetweenObjects = 0.14;

        /// <summary>
        /// Minimum arm reach
        /// </summary>
        private readonly double minArmReach = 0.53;

        /// <summary>
        /// Maximum arm reach
        /// </summary>
        private readonly double maxArmReach = 0.955;

        /// <summary>
        /// Minimum rotation angle
        /// </summary>
        private readonly double minRotate = -25.0;

        /// <summary>
        /// Maximum rotation angle
        /// </summary>
        private readonly double maxRotate = 25.0;

        /// <summary>
        /// Base orientation
        /// </summary>
        private readonly Quaternion basePose = Quaternion.FromAxisAngle(new AxisAngle(new Vector3(0, 0, 1), MathConstants.PIOverTwo));

        /// <summary>
        /// First location on table to place object
        /// </summary>
        private Pose startPoseOnTable;

        /// <summary>
        /// Reference arm position used for ordering pick place operations
        /// </summary>
        /// <returns></returns>
        private Vector3 armReferencePosition;

        /// <summary>
        /// Dimensions of CNC tray in X, Y and Z (Z is ignored because the tray
        /// is horizontal)
        /// </summary>
        private Vector3 cncLengths;

        /// <summary>
        /// Maximum number of objects that will be placed on CNC
        /// </summary>
        private int maxObjectsForPlace = 3;

        /// <summary>
        /// Fraction of overall CNC length that sets the CNC dimensions
        /// </summary>
        private double placeCenterExtentFrac = 0.1;


        /// <summary>
        /// maximum tries to generate a random place perturbation vector
        /// </summary>
        private int maxRandPlaceGenerateCount = 100;

        /// <summary>
        /// Dimension of rectangular extent around the place centers
        /// </summary>
        private Vector3 placeCenterExtent;

        /// <summary>
        /// Half the CNC tray dimension
        /// </summary>
        private Vector3 cncHalfLength;


        /// <summary>
        /// Candidate centers for place
        /// </summary>
        private List<Vector3> placeCenters;

        /// <summary>
        /// Generate positions on CNC tray for placement
        /// </summary>
        private Random rand;

        /// <summary>
        /// Initializes a new instance of the <see cref="MultiPicknPlace" /> class
        /// </summary>
        /// <param name="startPoseOnTable">Start pose on table for placing objects</param>
        /// <param name="armReferencePosition">Reference arm position used for ordering pick place operations</param>
        /// <param name="cncLengths">Dimensions of CNC tray (X and Y)</param>
        public MultiPicknPlace(
            Pose startPoseOnTable,
            Vector3 armReferencePosition,
            Vector3 cncLengths)
        {
            this.startPoseOnTable = startPoseOnTable;
            this.armReferencePosition = armReferencePosition;
            this.cncLengths = cncLengths;
            this.InitializeRandomCenters();
            this.rand = new Random(30);
        }


        /// <summary>
        /// Executes a set pick from CNC and place on table request. 
        /// Objects are placed in a row along the global X axis on the table
        /// </summary>
        /// <param name="pickFromCNC">Input array of requests</param>
        public void ExecutePickFromCNC(PickAndMove[] pickFromCNC, SinglePicknPlaceFSMContext pickFromCNCContext)
        {
            pickFromCNCContext.PickList = pickFromCNC.OrderByDescending(k => k.PickFromPose.Position.Y).ToArray();

            pickFromCNCContext.PlaceList = new PlaceAndMove[pickFromCNCContext.PickList.Length];

            Vector3 currentPosition = new Vector3(this.startPoseOnTable.Position);
            Vector3 lastPosition = currentPosition;

            for (int i = 0; i < pickFromCNCContext.PickList.Length; ++i)
            {
                PickAndMove pick = pickFromCNCContext.PickList[i];
                double widthBy2 = pick.ManipulatedObjectDescription.WidthInM / 2.0;

                if (i > 0)
                {
                    currentPosition = new Vector3(currentPosition.X - widthBy2, currentPosition.Y, currentPosition.Z);

                    if (Math.Abs(lastPosition.X - currentPosition.X) < this.minRequiredSpaceBetweenObjects)
                    {
                        currentPosition = new Vector3(lastPosition.X - this.minRequiredSpaceBetweenObjects, currentPosition.Y, currentPosition.Z);
                    }
                }

                Pose pose = new Pose(new Vector3(currentPosition.X, currentPosition.Y, currentPosition.Z + (pick.ManipulatedObjectDescription.HeightInM / 2.0)), this.startPoseOnTable.Orientation);
                lastPosition = currentPosition;
                
                pickFromCNCContext.PlaceList[i] = new PlaceAndMove(
                    pick.ManipulatedObjectDescription,
                    pose,
                    pick.MoveToPose);

                currentPosition = new Vector3(currentPosition.X - widthBy2, currentPosition.Y, currentPosition.Z);
            }
        }

        /// <summary>
        /// Executes a multiple object place request from the intermediate place position back
        /// to the CNC.
        /// Objects are picked from the table and placed at pseudo random locations on the CNC tray
        /// </summary>
        /// <param name="placeToCNC">Input array of requests</param>
        /// <param name="pickFromCNCContext">Sequencer context for the previous multiple pick from CNC that placed object on table</param>
        /// <param name="placeToCNCContext">Sequencer context for teh planned place from table to CNC</param>
        public void ExecutePlaceToCNC(PlaceAndMove[] placeToCNC, SinglePicknPlaceFSMContext pickFromCNCContext, SinglePicknPlaceFSMContext placeToCNCContext)
        {
            // Construct the ordered place positions on the tray
            Vector3 centre = placeToCNC[0].PlacePose.Position;
            Quaternion requestOrient = placeToCNC[0].PlacePose.Orientation;
            PlaceAndMove request = placeToCNC[0];

            double minClearance;
            double minYClearance;
            Vector3[] deviationFromTrayCenter;

            // Generate a set of target locations on the CNC. 
            // Run a couple of heuristsic checks to confirm objects are sufficiently well spaced
            // to avoid collisions. If the checks fail just try another position. In practice this 
            // converges after a few iterations.
            do
            {
                deviationFromTrayCenter = this.GenerateRandomPositionSet(centre, pickFromCNCContext.PickList.Length);
                minClearance = this.minRequiredSpaceBetweenObjects;
                minYClearance = this.minRequiredYSpaceBetweenObjects;

                for (int i = 0; i < deviationFromTrayCenter.Length; ++i)
                {
                    for (int j = 0; j < i; ++j)
                    {
                        double d = (deviationFromTrayCenter[i] - deviationFromTrayCenter[j]).Length();
                        double dy = Math.Abs(deviationFromTrayCenter[i].Y - deviationFromTrayCenter[j].Y);
                        minClearance = Math.Min(minClearance, d);
                        minYClearance = Math.Min(dy, minYClearance);
                    }
                }

            } while (minClearance < this.minRequiredSpaceBetweenObjects || minYClearance < this.minRequiredYSpaceBetweenObjects);


            Vector3[] deviationFromTrayCenterSort = deviationFromTrayCenter.OrderBy(k => k.Y).ToArray();
            PlaceAndMove[] lastPlaceOnTable = pickFromCNCContext.PlaceList;

            // The heuristic to avoid collisions is to place objects in the reverse order they were placed.
            placeToCNCContext.PickList = new PickAndMove[lastPlaceOnTable.Length];
            placeToCNCContext.PlaceList = new PlaceAndMove[lastPlaceOnTable.Length];
            lastPlaceOnTable = pickFromCNCContext.PlaceList.Reverse().ToArray();

            for (int i = 0; i < lastPlaceOnTable.Length; ++i)
            {
                PlaceAndMove table = lastPlaceOnTable[i];
                placeToCNCContext.PickList[i] = new PickAndMove(table.ManipulatedObjectDescription, table.PlacePose, table.RetractPose, PicknPlaceModeEnum.Baseline);
                Vector3 placePos = new Vector3(deviationFromTrayCenterSort[i].X, deviationFromTrayCenterSort[i].Y, deviationFromTrayCenterSort[i].Z + table.ManipulatedObjectDescription.HeightInM / 2.0);
                placeToCNCContext.PlaceList[i] = new PlaceAndMove(table.ManipulatedObjectDescription, new Pose(placePos, this.GenerateRandomOrientation()), request.RetractPose);
            }
        }

        /// <summary>
        /// Generate a set of random positions on the CNC around a center point
        /// </summary>
        /// <param name="center">Center to use</param>
        /// <param name="count">Number of random position in set</param>
        /// <returns>the candidate set</returns>
        private Vector3[] GenerateRandomPositionSet(Vector3 center, int count)
        {
            Vector3[] positionSet = new Vector3[count];
            HashSet<int> usedCluster = new HashSet<int>();

            for (int i = 0; i < count; ++i)
            {
                Vector3? nextPosition = null;

                do
                {
                    int clusterIdx;
                    do
                    {
                        clusterIdx = this.rand.Next(this.maxObjectsForPlace) + i * this.maxObjectsForPlace;
                    } while (true == usedCluster.Contains(clusterIdx));

                    usedCluster.Add(clusterIdx);
                    nextPosition = this.GenerateRandomPosition(center, this.placeCenters[clusterIdx]);
                } while (false == nextPosition.HasValue);

                positionSet[i] = nextPosition.Value;
            }

            return positionSet;
        }

        /// <summary>
        /// Generate a random position perturbation for CNC tray in global coordinates
        /// </summary>
        /// <param name="cncCenter">Current CNC centere in global coordinates</param>
        /// <param name="perturbCenter">Center of perturbation relative to tray center</param>
        /// <returns>Perturbation in X Y plane</returns>
        private Vector3? GenerateRandomPosition(Vector3 cncCenter, Vector3 perturbCenter)
        {
            Vector3? randVec;
            Vector3 perturb;
            double dist;
            int cnt = 0;

            do
            {
                double x = this.placeCenterExtent.X * (2.0 * this.rand.NextDouble() - 1.0);
                double y = this.placeCenterExtent.Y * (2.0 * this.rand.NextDouble() - 1.0);
                perturb = perturbCenter + new Vector3(x, y, 0);
                randVec = cncCenter + perturb;
                dist = randVec.Value.Length();
                ++cnt;
            }
            while (
                cnt < this.maxRandPlaceGenerateCount && 
                (dist <= this.minArmReach || 
                dist >= this.maxArmReach || 
                perturb.X < -this.cncHalfLength.X || 
                perturb.X > this.cncHalfLength.X || 
                perturb.Y < -this.cncHalfLength.Y ||
                perturb.Y > this.cncHalfLength.Y));

            if (cnt >= this.maxRandPlaceGenerateCount)
            {
                randVec = null;
            }

            return randVec;
        }

        /// <summary>
        /// Generate a random orientation
        /// </summary>
        /// <returns>Generated orientation</returns>
        private Quaternion GenerateRandomOrientation()
        {
            double angle = (this.maxRotate - this.minRotate) * this.rand.NextDouble() + this.minRotate;

            Quaternion q = Quaternion.FromAxisAngle(0, 0, 1, angle * MathConstants.Degrees2Radians);
            q = q * this.basePose;
            return q;
        }

        /// <summary>
        /// Initialize a set of well separated place cluster centers on the CNC tray
        /// </summary>
        private void InitializeRandomCenters()
        {
            this.placeCenters = new List<Vector3>();
            this.placeCenterExtent = this.cncLengths * this.placeCenterExtentFrac;
            this.cncHalfLength = this.cncLengths * 0.5;
            Vector3 workingLength = this.cncHalfLength * (1.0 - this.placeCenterExtentFrac);

            double end = Math.Floor(this.maxObjectsForPlace / 2.0);
            for (double y = -end; y <= end; ++y)
            {
                for (double x = -end; x <= end; ++x)
                {
                    this.placeCenters.Add(new Vector3(x * workingLength.X, y * workingLength.Y, 0));
                }
            }
        }
    }
}
